<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import java.awt.geom.Point2D;<a name="line.3"></a>
<span class="sourceLineNo">004</span><a name="line.4"></a>
<span class="sourceLineNo">005</span>import org.apache.commons.math3.analysis.polynomials.PolynomialFunction;<a name="line.5"></a>
<span class="sourceLineNo">006</span>import org.apache.commons.math3.analysis.solvers.NewtonRaphsonSolver;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import org.apache.commons.math3.analysis.solvers.UnivariateDifferentiableSolver;<a name="line.7"></a>
<span class="sourceLineNo">008</span>import org.apache.commons.math3.linear.MatrixUtils;<a name="line.8"></a>
<span class="sourceLineNo">009</span>import org.apache.commons.math3.linear.RealMatrix;<a name="line.9"></a>
<span class="sourceLineNo">010</span>import org.apache.commons.math3.linear.RealVector;<a name="line.10"></a>
<span class="sourceLineNo">011</span><a name="line.11"></a>
<span class="sourceLineNo">012</span>import imagingbook.calibration.zhang.util.MathUtil;<a name="line.12"></a>
<span class="sourceLineNo">013</span>import imagingbook.lib.math.Matrix;<a name="line.13"></a>
<span class="sourceLineNo">014</span><a name="line.14"></a>
<span class="sourceLineNo">015</span><a name="line.15"></a>
<span class="sourceLineNo">016</span>/**<a name="line.16"></a>
<span class="sourceLineNo">017</span> * A camera model with parameters as specified in Zhang's paper.<a name="line.17"></a>
<span class="sourceLineNo">018</span> * @author WB<a name="line.18"></a>
<span class="sourceLineNo">019</span> */<a name="line.19"></a>
<span class="sourceLineNo">020</span>public class Camera {<a name="line.20"></a>
<span class="sourceLineNo">021</span>        <a name="line.21"></a>
<span class="sourceLineNo">022</span>        /**  <a name="line.22"></a>
<span class="sourceLineNo">023</span>         * The camera's inner transformation matrix:<a name="line.23"></a>
<span class="sourceLineNo">024</span>         * | alpha  gamma  uc |<a name="line.24"></a>
<span class="sourceLineNo">025</span>         * |     0   beta  vc |<a name="line.25"></a>
<span class="sourceLineNo">026</span>         */<a name="line.26"></a>
<span class="sourceLineNo">027</span>        private final double[][] A;     // 2 x 3<a name="line.27"></a>
<span class="sourceLineNo">028</span>        private final double[] K;       // the vector of lens distortion coefficients<a name="line.28"></a>
<span class="sourceLineNo">029</span>                <a name="line.29"></a>
<span class="sourceLineNo">030</span>        // for the standard Zhang camera<a name="line.30"></a>
<span class="sourceLineNo">031</span>        public Camera(double alpha, double beta, double gamma, double uc, double vc, double k0, double k1) {<a name="line.31"></a>
<span class="sourceLineNo">032</span>                this.A = makeA(alpha, beta, gamma, uc, vc);<a name="line.32"></a>
<span class="sourceLineNo">033</span>                this.K = new double[] {k0, k1};<a name="line.33"></a>
<span class="sourceLineNo">034</span>        }<a name="line.34"></a>
<span class="sourceLineNo">035</span>        <a name="line.35"></a>
<span class="sourceLineNo">036</span>        /**<a name="line.36"></a>
<span class="sourceLineNo">037</span>         * Creates a standard camera from a vector of intrinsic parameters <a name="line.37"></a>
<span class="sourceLineNo">038</span>         * @param s a vector of intrinsic camera parameters (alpha, beta, gamma, uc, vc, k0, k1).<a name="line.38"></a>
<span class="sourceLineNo">039</span>         */<a name="line.39"></a>
<span class="sourceLineNo">040</span>        public Camera (double[] s) {<a name="line.40"></a>
<span class="sourceLineNo">041</span>                //s = (alpha, beta, gamma, uc, vc, k0, k1);     <a name="line.41"></a>
<span class="sourceLineNo">042</span>                this.A = makeA(s[0], s[1], s[2], s[3], s[4]);<a name="line.42"></a>
<span class="sourceLineNo">043</span>                this.K = new double[] {s[5], s[6]};<a name="line.43"></a>
<span class="sourceLineNo">044</span>        }<a name="line.44"></a>
<span class="sourceLineNo">045</span>                <a name="line.45"></a>
<span class="sourceLineNo">046</span>        /**<a name="line.46"></a>
<span class="sourceLineNo">047</span>         * Creates a standard camera from a transformation matrix and a vector<a name="line.47"></a>
<span class="sourceLineNo">048</span>         * of lens distortion coefficients.<a name="line.48"></a>
<span class="sourceLineNo">049</span>         * @param A the (min.) 2 x 3 matrix holding the intrinsic camera parameters<a name="line.49"></a>
<span class="sourceLineNo">050</span>         * @param K the radial distortion coefficients k0, k1, ... (may be {@code null})<a name="line.50"></a>
<span class="sourceLineNo">051</span>         */<a name="line.51"></a>
<span class="sourceLineNo">052</span>        public Camera(RealMatrix A, double[] K) {<a name="line.52"></a>
<span class="sourceLineNo">053</span>                this.K = (K == null) ? new double[0] : K.clone();<a name="line.53"></a>
<span class="sourceLineNo">054</span>                this.A = A.getSubMatrix(0, 1, 0, 2).getData();<a name="line.54"></a>
<span class="sourceLineNo">055</span>        }<a name="line.55"></a>
<span class="sourceLineNo">056</span>        <a name="line.56"></a>
<span class="sourceLineNo">057</span>        /**<a name="line.57"></a>
<span class="sourceLineNo">058</span>         * Creates a simple pinhole camera (with no distortion whatsoever).<a name="line.58"></a>
<span class="sourceLineNo">059</span>         * @param f the camera's focal length (in pixel units).<a name="line.59"></a>
<span class="sourceLineNo">060</span>         * @param uc the x-position of the optical axis intersecting the image plane (in pixel units)<a name="line.60"></a>
<span class="sourceLineNo">061</span>         * @param vc the y-position of the optical axis intersecting the image plane (in pixel units)<a name="line.61"></a>
<span class="sourceLineNo">062</span>         */<a name="line.62"></a>
<span class="sourceLineNo">063</span>        public Camera(double f, double uc, double vc) {<a name="line.63"></a>
<span class="sourceLineNo">064</span>                this.K = new double[0];<a name="line.64"></a>
<span class="sourceLineNo">065</span>                this.A = makeA(f, f, 0, uc, vc);<a name="line.65"></a>
<span class="sourceLineNo">066</span>        }<a name="line.66"></a>
<span class="sourceLineNo">067</span>        <a name="line.67"></a>
<span class="sourceLineNo">068</span>        // --------------------------------------------------------------------------<a name="line.68"></a>
<span class="sourceLineNo">069</span>        <a name="line.69"></a>
<span class="sourceLineNo">070</span>        private double[][] makeA(double alpha, double beta, double gamma, double uc, double vc) {<a name="line.70"></a>
<span class="sourceLineNo">071</span>                return new double[][] {<a name="line.71"></a>
<span class="sourceLineNo">072</span>                                {alpha, gamma, uc},<a name="line.72"></a>
<span class="sourceLineNo">073</span>                                {    0,  beta, vc}};<a name="line.73"></a>
<span class="sourceLineNo">074</span>        }<a name="line.74"></a>
<span class="sourceLineNo">075</span>        <a name="line.75"></a>
<span class="sourceLineNo">076</span>        // P is assumed to be a X/Y point in the Z = 0 plane<a name="line.76"></a>
<span class="sourceLineNo">077</span>        /**<a name="line.77"></a>
<span class="sourceLineNo">078</span>         * Projects the X/Y world point (in the Z = 0 plane)<a name="line.78"></a>
<span class="sourceLineNo">079</span>         * to image coordinates under the given camera view (extrinsic transformation parameters).<a name="line.79"></a>
<span class="sourceLineNo">080</span>         * @param view the extrinsic transformation parameters<a name="line.80"></a>
<span class="sourceLineNo">081</span>         * @param P a single X/Y world point (with Z = 0)<a name="line.81"></a>
<span class="sourceLineNo">082</span>         * @return the projected 2D image coordinates<a name="line.82"></a>
<span class="sourceLineNo">083</span>         */<a name="line.83"></a>
<span class="sourceLineNo">084</span>        public double[] project(ViewTransform view, Point2D P) {<a name="line.84"></a>
<span class="sourceLineNo">085</span>                double[] XY0 = new double[] {P.getX(), P.getY(), 0}; <a name="line.85"></a>
<span class="sourceLineNo">086</span>                return this.project(view, XY0);<a name="line.86"></a>
<span class="sourceLineNo">087</span>        }<a name="line.87"></a>
<span class="sourceLineNo">088</span>        <a name="line.88"></a>
<span class="sourceLineNo">089</span>        /**<a name="line.89"></a>
<span class="sourceLineNo">090</span>         * Projects the X/Y world points (all in the Z = 0 plane)<a name="line.90"></a>
<span class="sourceLineNo">091</span>         * to image coordinates under the given camera view (extrinsic transformation parameters).<a name="line.91"></a>
<span class="sourceLineNo">092</span>         * @param view the extrinsic transformation parameters<a name="line.92"></a>
<span class="sourceLineNo">093</span>         * @param PP a set of X/Y world points (with Z = 0)<a name="line.93"></a>
<span class="sourceLineNo">094</span>         * @return the projected 2D image coordinates<a name="line.94"></a>
<span class="sourceLineNo">095</span>         */<a name="line.95"></a>
<span class="sourceLineNo">096</span>        public Point2D[] project(ViewTransform view, Point2D[] PP) {<a name="line.96"></a>
<span class="sourceLineNo">097</span>                Point2D[] imagePoints = new Point2D[PP.length];<a name="line.97"></a>
<span class="sourceLineNo">098</span>                for (int j = 0; j &lt; PP.length; j++) {<a name="line.98"></a>
<span class="sourceLineNo">099</span>                        double[] uv = project(view, PP[j]);<a name="line.99"></a>
<span class="sourceLineNo">100</span>                        imagePoints[j] = MathUtil.toPoint2D(uv);<a name="line.100"></a>
<span class="sourceLineNo">101</span>                }<a name="line.101"></a>
<span class="sourceLineNo">102</span>                return imagePoints;<a name="line.102"></a>
<span class="sourceLineNo">103</span>        }<a name="line.103"></a>
<span class="sourceLineNo">104</span>        <a name="line.104"></a>
<span class="sourceLineNo">105</span>        /**<a name="line.105"></a>
<span class="sourceLineNo">106</span>         * Projects the given 3D point onto the sensor plane of this camera<a name="line.106"></a>
<span class="sourceLineNo">107</span>         * for the provided extrinsic view parameters.<a name="line.107"></a>
<span class="sourceLineNo">108</span>         * <a name="line.108"></a>
<span class="sourceLineNo">109</span>         * @param view the extrinsic camera (view) parameters<a name="line.109"></a>
<span class="sourceLineNo">110</span>         * @param XYZ a point in 3D world coordinates<a name="line.110"></a>
<span class="sourceLineNo">111</span>         * @return the 2D sensor coordinates of the projected point<a name="line.111"></a>
<span class="sourceLineNo">112</span>         */<a name="line.112"></a>
<span class="sourceLineNo">113</span>        public double[] project(ViewTransform view, double[] XYZ) {<a name="line.113"></a>
<span class="sourceLineNo">114</span>                // map to the ideal projection plane (f = 1)<a name="line.114"></a>
<span class="sourceLineNo">115</span>                double[] xy = projectNormalized(view, XYZ);<a name="line.115"></a>
<span class="sourceLineNo">116</span>                // apply radial lens distortion to the ideal projection<a name="line.116"></a>
<span class="sourceLineNo">117</span>                double[] xyd = warp(xy);<a name="line.117"></a>
<span class="sourceLineNo">118</span>                // apply the intrinsic camera transformation:<a name="line.118"></a>
<span class="sourceLineNo">119</span>                double[] uv = mapToSensorPlane(xyd);<a name="line.119"></a>
<span class="sourceLineNo">120</span>                return uv;<a name="line.120"></a>
<span class="sourceLineNo">121</span>        }<a name="line.121"></a>
<span class="sourceLineNo">122</span>        <a name="line.122"></a>
<span class="sourceLineNo">123</span>        <a name="line.123"></a>
<span class="sourceLineNo">124</span>        /**<a name="line.124"></a>
<span class="sourceLineNo">125</span>         * Projects the given 3D point to ideal projection coordinates<a name="line.125"></a>
<span class="sourceLineNo">126</span>         * for the provided extrinsic view parameters. The world point is<a name="line.126"></a>
<span class="sourceLineNo">127</span>         * specified as a 2D coordinate in the Z = 0 plane.<a name="line.127"></a>
<span class="sourceLineNo">128</span>         * <a name="line.128"></a>
<span class="sourceLineNo">129</span>         * @param view the extrinsic camera (view) parameters<a name="line.129"></a>
<span class="sourceLineNo">130</span>         * @param P a point in 3D world coordinates (Z = 0)<a name="line.130"></a>
<span class="sourceLineNo">131</span>         * @return the 2D ideal projection<a name="line.131"></a>
<span class="sourceLineNo">132</span>         */<a name="line.132"></a>
<span class="sourceLineNo">133</span>        public double[] projectNormalized(ViewTransform view, Point2D P) {<a name="line.133"></a>
<span class="sourceLineNo">134</span>                double[] XY0 = {P.getX(), P.getY(), 0};<a name="line.134"></a>
<span class="sourceLineNo">135</span>                return projectNormalized(view, XY0);<a name="line.135"></a>
<span class="sourceLineNo">136</span>        }<a name="line.136"></a>
<span class="sourceLineNo">137</span>        <a name="line.137"></a>
<span class="sourceLineNo">138</span>        /**<a name="line.138"></a>
<span class="sourceLineNo">139</span>         * Projects the given 3D point to ideal projection coordinates<a name="line.139"></a>
<span class="sourceLineNo">140</span>         * for the provided extrinsic view parameters.<a name="line.140"></a>
<span class="sourceLineNo">141</span>         * <a name="line.141"></a>
<span class="sourceLineNo">142</span>         * @param view the extrinsic camera (view) parameters<a name="line.142"></a>
<span class="sourceLineNo">143</span>         * @param XYZ a point in 3D world coordinates<a name="line.143"></a>
<span class="sourceLineNo">144</span>         * @return the 2D ideal projection<a name="line.144"></a>
<span class="sourceLineNo">145</span>         */<a name="line.145"></a>
<span class="sourceLineNo">146</span>        public double[] projectNormalized(ViewTransform view, double[] XYZ) {<a name="line.146"></a>
<span class="sourceLineNo">147</span>                double[] XYZc = view.applyTo(XYZ);<a name="line.147"></a>
<span class="sourceLineNo">148</span>                // Compute normalized projection coordinates (f = 1):<a name="line.148"></a>
<span class="sourceLineNo">149</span>                final double x = XYZc[0] / XYZc[2];<a name="line.149"></a>
<span class="sourceLineNo">150</span>                final double y = XYZc[1] / XYZc[2];<a name="line.150"></a>
<span class="sourceLineNo">151</span>                return new double[] {x, y};<a name="line.151"></a>
<span class="sourceLineNo">152</span>        }<a name="line.152"></a>
<span class="sourceLineNo">153</span>        <a name="line.153"></a>
<span class="sourceLineNo">154</span>        // not used in this form, just for symmetry<a name="line.154"></a>
<span class="sourceLineNo">155</span>        private double warp(double r) {<a name="line.155"></a>
<span class="sourceLineNo">156</span>                return r * (1 + D(r));<a name="line.156"></a>
<span class="sourceLineNo">157</span>        }<a name="line.157"></a>
<span class="sourceLineNo">158</span>        <a name="line.158"></a>
<span class="sourceLineNo">159</span>        /**<a name="line.159"></a>
<span class="sourceLineNo">160</span>         * Applies radial distortion to a point in the ideal 2D projection.<a name="line.160"></a>
<span class="sourceLineNo">161</span>         * @param xy a 2D point in the ideal projection<a name="line.161"></a>
<span class="sourceLineNo">162</span>         * @return the lens-distorted position in the ideal projection<a name="line.162"></a>
<span class="sourceLineNo">163</span>         */<a name="line.163"></a>
<span class="sourceLineNo">164</span>        public double[] warp(double[] xy) {<a name="line.164"></a>
<span class="sourceLineNo">165</span>                final double x = xy[0];<a name="line.165"></a>
<span class="sourceLineNo">166</span>                final double y = xy[1];<a name="line.166"></a>
<span class="sourceLineNo">167</span>                final double r = Math.sqrt(x * x + y * y);<a name="line.167"></a>
<span class="sourceLineNo">168</span>                double d = (1 + D(r));<a name="line.168"></a>
<span class="sourceLineNo">169</span>                return new double[] {d * x, d * y};<a name="line.169"></a>
<span class="sourceLineNo">170</span>        }<a name="line.170"></a>
<span class="sourceLineNo">171</span>        <a name="line.171"></a>
<span class="sourceLineNo">172</span>        /**<a name="line.172"></a>
<span class="sourceLineNo">173</span>         * Inverse radial distortion function. Finds the original<a name="line.173"></a>
<span class="sourceLineNo">174</span>         * (undistorted) radius r from the distorted radius R, both<a name="line.174"></a>
<span class="sourceLineNo">175</span>         * measured from the center = (0,0) of the ideal projection.<a name="line.175"></a>
<span class="sourceLineNo">176</span>         * Finds r as the root of the polynomial<a name="line.176"></a>
<span class="sourceLineNo">177</span>         * p(r) = - R + r + k0 * r^3 + k1 * r^5,<a name="line.177"></a>
<span class="sourceLineNo">178</span>         * where R is constant, by using a Newton-Raphson solver.<a name="line.178"></a>
<span class="sourceLineNo">179</span>         * @param R the distorted radius<a name="line.179"></a>
<span class="sourceLineNo">180</span>         * @return the undistorted radius<a name="line.180"></a>
<span class="sourceLineNo">181</span>         */<a name="line.181"></a>
<span class="sourceLineNo">182</span>        public double unwarp(double R) {<a name="line.182"></a>
<span class="sourceLineNo">183</span>                double k0 = K[0];<a name="line.183"></a>
<span class="sourceLineNo">184</span>                double k1 = K[1];<a name="line.184"></a>
<span class="sourceLineNo">185</span>                double[] coefficients = {-R, 1, 0, k0, 0, k1};<a name="line.185"></a>
<span class="sourceLineNo">186</span>                PolynomialFunction p = new PolynomialFunction(coefficients);<a name="line.186"></a>
<span class="sourceLineNo">187</span>                UnivariateDifferentiableSolver solver = new NewtonRaphsonSolver();<a name="line.187"></a>
<span class="sourceLineNo">188</span>                double rInit = R;<a name="line.188"></a>
<span class="sourceLineNo">189</span>                int maxEval = 20;<a name="line.189"></a>
<span class="sourceLineNo">190</span>                double r = solver.solve(maxEval, p, rInit);<a name="line.190"></a>
<span class="sourceLineNo">191</span>//              System.out.format("** solver iterations = %d\n", solver.getEvaluations());<a name="line.191"></a>
<span class="sourceLineNo">192</span>                return r;<a name="line.192"></a>
<span class="sourceLineNo">193</span>        }<a name="line.193"></a>
<span class="sourceLineNo">194</span>        <a name="line.194"></a>
<span class="sourceLineNo">195</span>        /**<a name="line.195"></a>
<span class="sourceLineNo">196</span>         * Applies inverse radial distortion to a given point in the ideal image plane.<a name="line.196"></a>
<span class="sourceLineNo">197</span>         * @param xyd a distorted 2D point in the ideal image plane<a name="line.197"></a>
<span class="sourceLineNo">198</span>         * @return the undistorted point<a name="line.198"></a>
<span class="sourceLineNo">199</span>         */<a name="line.199"></a>
<span class="sourceLineNo">200</span>        public double[] unwarp(double[] xyd) {<a name="line.200"></a>
<span class="sourceLineNo">201</span>                final double xd = xyd[0];<a name="line.201"></a>
<span class="sourceLineNo">202</span>                final double yd = xyd[1];<a name="line.202"></a>
<span class="sourceLineNo">203</span>                final double R = Math.sqrt(xd * xd + yd * yd);  // distorted radius<a name="line.203"></a>
<span class="sourceLineNo">204</span>                final double r = unwarp(R);                                             // undistorted radius<a name="line.204"></a>
<span class="sourceLineNo">205</span>                final double s = r / R;<a name="line.205"></a>
<span class="sourceLineNo">206</span>                return new double[] {s * xd, s * yd};<a name="line.206"></a>
<span class="sourceLineNo">207</span>        }<a name="line.207"></a>
<span class="sourceLineNo">208</span>        <a name="line.208"></a>
<span class="sourceLineNo">209</span><a name="line.209"></a>
<span class="sourceLineNo">210</span>        /**<a name="line.210"></a>
<span class="sourceLineNo">211</span>         * Radial distortion function, to be applied in the form r' = r * (1 + D(r))<a name="line.211"></a>
<span class="sourceLineNo">212</span>         * to points in the ideal projection plane.<a name="line.212"></a>
<span class="sourceLineNo">213</span>         * Distortion coefficients k0, k1 are a property of the enclosing {@link Camera}.<a name="line.213"></a>
<span class="sourceLineNo">214</span>         * @param r the original radius of a point in the ideal projection plane<a name="line.214"></a>
<span class="sourceLineNo">215</span>         * @return the pos/neg deviation for the given radius<a name="line.215"></a>
<span class="sourceLineNo">216</span>         */<a name="line.216"></a>
<span class="sourceLineNo">217</span>        public double D(double r) {<a name="line.217"></a>
<span class="sourceLineNo">218</span>                final double k0 = (K.length &gt; 0) ? K[0] : 0;<a name="line.218"></a>
<span class="sourceLineNo">219</span>                final double k1 = (K.length &gt; 1) ? K[1] : 0;<a name="line.219"></a>
<span class="sourceLineNo">220</span>                final double r2 = r * r;<a name="line.220"></a>
<span class="sourceLineNo">221</span>                return (k0 + k1 * r2) * r2;             // D(r) = k0 * r^2 + k1 * r^4<a name="line.221"></a>
<span class="sourceLineNo">222</span>        }<a name="line.222"></a>
<span class="sourceLineNo">223</span>        <a name="line.223"></a>
<span class="sourceLineNo">224</span>        <a name="line.224"></a>
<span class="sourceLineNo">225</span>        /**<a name="line.225"></a>
<span class="sourceLineNo">226</span>         * Maps from the ideal projection plane to sensor coordinates,<a name="line.226"></a>
<span class="sourceLineNo">227</span>         * using the camera's intrinsic parameters.<a name="line.227"></a>
<span class="sourceLineNo">228</span>         * @param xyd a 2D point on the ideal projection plane<a name="line.228"></a>
<span class="sourceLineNo">229</span>         * @return the resulting 2D sensor coordinate<a name="line.229"></a>
<span class="sourceLineNo">230</span>         */<a name="line.230"></a>
<span class="sourceLineNo">231</span>        public double[] mapToSensorPlane(double[] xyd) {<a name="line.231"></a>
<span class="sourceLineNo">232</span>                final double x = xyd[0];<a name="line.232"></a>
<span class="sourceLineNo">233</span>                final double y = xyd[1];<a name="line.233"></a>
<span class="sourceLineNo">234</span>                final double u = A[0][0] * x + A[0][1] * y + A[0][2];<a name="line.234"></a>
<span class="sourceLineNo">235</span>                final double v =               A[1][1] * y + A[1][2];<a name="line.235"></a>
<span class="sourceLineNo">236</span>                return new double[] {u, v};     <a name="line.236"></a>
<span class="sourceLineNo">237</span>        }<a name="line.237"></a>
<span class="sourceLineNo">238</span>        <a name="line.238"></a>
<span class="sourceLineNo">239</span>        // -------------------------------------------------------------------<a name="line.239"></a>
<span class="sourceLineNo">240</span>        <a name="line.240"></a>
<span class="sourceLineNo">241</span>        /**<a name="line.241"></a>
<span class="sourceLineNo">242</span>         * Returns the camera's inner parameters as a vector <a name="line.242"></a>
<span class="sourceLineNo">243</span>         * (alpha, beta, gamma, uc, vc, k0, k1).<a name="line.243"></a>
<span class="sourceLineNo">244</span>         * @return the camera's inner parameters<a name="line.244"></a>
<span class="sourceLineNo">245</span>         */<a name="line.245"></a>
<span class="sourceLineNo">246</span>        public double[] getParameterVector() {<a name="line.246"></a>
<span class="sourceLineNo">247</span>                return new double[] <a name="line.247"></a>
<span class="sourceLineNo">248</span>                                {getAlpha(), getBeta(), getGamma(), getUc(), getVc(), K[0], K[1]};      <a name="line.248"></a>
<span class="sourceLineNo">249</span>        }<a name="line.249"></a>
<span class="sourceLineNo">250</span><a name="line.250"></a>
<span class="sourceLineNo">251</span>        /**<a name="line.251"></a>
<span class="sourceLineNo">252</span>         * Returns the camera's alpha value.<a name="line.252"></a>
<span class="sourceLineNo">253</span>         * @return alpha<a name="line.253"></a>
<span class="sourceLineNo">254</span>         */<a name="line.254"></a>
<span class="sourceLineNo">255</span>        public double getAlpha() {<a name="line.255"></a>
<span class="sourceLineNo">256</span>                return A[0][0];<a name="line.256"></a>
<span class="sourceLineNo">257</span>        }<a name="line.257"></a>
<span class="sourceLineNo">258</span><a name="line.258"></a>
<span class="sourceLineNo">259</span>        /**<a name="line.259"></a>
<span class="sourceLineNo">260</span>         * Returns the camera's beta value.<a name="line.260"></a>
<span class="sourceLineNo">261</span>         * @return beta<a name="line.261"></a>
<span class="sourceLineNo">262</span>         */<a name="line.262"></a>
<span class="sourceLineNo">263</span>        public double getBeta() {<a name="line.263"></a>
<span class="sourceLineNo">264</span>                return A[1][1];<a name="line.264"></a>
<span class="sourceLineNo">265</span>        }<a name="line.265"></a>
<span class="sourceLineNo">266</span><a name="line.266"></a>
<span class="sourceLineNo">267</span>        /**<a name="line.267"></a>
<span class="sourceLineNo">268</span>         * Returns the camera's gamma value.<a name="line.268"></a>
<span class="sourceLineNo">269</span>         * @return gamma<a name="line.269"></a>
<span class="sourceLineNo">270</span>         */<a name="line.270"></a>
<span class="sourceLineNo">271</span>        public double getGamma() {<a name="line.271"></a>
<span class="sourceLineNo">272</span>                return A[0][1];<a name="line.272"></a>
<span class="sourceLineNo">273</span>        }<a name="line.273"></a>
<span class="sourceLineNo">274</span><a name="line.274"></a>
<span class="sourceLineNo">275</span>        /**<a name="line.275"></a>
<span class="sourceLineNo">276</span>         * Returns the camera's uc value.<a name="line.276"></a>
<span class="sourceLineNo">277</span>         * @return uc<a name="line.277"></a>
<span class="sourceLineNo">278</span>         */<a name="line.278"></a>
<span class="sourceLineNo">279</span>        public double getUc() {<a name="line.279"></a>
<span class="sourceLineNo">280</span>                return A[0][2];<a name="line.280"></a>
<span class="sourceLineNo">281</span>        }<a name="line.281"></a>
<span class="sourceLineNo">282</span><a name="line.282"></a>
<span class="sourceLineNo">283</span>        /**<a name="line.283"></a>
<span class="sourceLineNo">284</span>         * Returns the camera's vc value.<a name="line.284"></a>
<span class="sourceLineNo">285</span>         * @return vc<a name="line.285"></a>
<span class="sourceLineNo">286</span>         */<a name="line.286"></a>
<span class="sourceLineNo">287</span>        public double getVc() {<a name="line.287"></a>
<span class="sourceLineNo">288</span>                return A[1][2];<a name="line.288"></a>
<span class="sourceLineNo">289</span>        }<a name="line.289"></a>
<span class="sourceLineNo">290</span><a name="line.290"></a>
<span class="sourceLineNo">291</span>        /**<a name="line.291"></a>
<span class="sourceLineNo">292</span>         * Returns the camera's lens distortion coefficients.<a name="line.292"></a>
<span class="sourceLineNo">293</span>         * @return the vector of lens distortion coefficients<a name="line.293"></a>
<span class="sourceLineNo">294</span>         */<a name="line.294"></a>
<span class="sourceLineNo">295</span>        public double[] getK() {<a name="line.295"></a>
<span class="sourceLineNo">296</span>                return K;<a name="line.296"></a>
<span class="sourceLineNo">297</span>        }<a name="line.297"></a>
<span class="sourceLineNo">298</span>        <a name="line.298"></a>
<span class="sourceLineNo">299</span>        /**<a name="line.299"></a>
<span class="sourceLineNo">300</span>         * Returns a copy of the camera's inner transformation matrix with contents<a name="line.300"></a>
<span class="sourceLineNo">301</span>         * &lt;pre&gt; <a name="line.301"></a>
<span class="sourceLineNo">302</span>         *    alpha  gamma  uc<a name="line.302"></a>
<span class="sourceLineNo">303</span>         *        0   beta  vc <a name="line.303"></a>
<span class="sourceLineNo">304</span>         * &lt;/pre&gt;<a name="line.304"></a>
<span class="sourceLineNo">305</span>         * <a name="line.305"></a>
<span class="sourceLineNo">306</span>         * @return the camara's inner transformation matrix (2 x 3)<a name="line.306"></a>
<span class="sourceLineNo">307</span>         */<a name="line.307"></a>
<span class="sourceLineNo">308</span>        public RealMatrix getA() {<a name="line.308"></a>
<span class="sourceLineNo">309</span>                return MatrixUtils.createRealMatrix(A);<a name="line.309"></a>
<span class="sourceLineNo">310</span>        }<a name="line.310"></a>
<span class="sourceLineNo">311</span>        <a name="line.311"></a>
<span class="sourceLineNo">312</span>        <a name="line.312"></a>
<span class="sourceLineNo">313</span>        /**<a name="line.313"></a>
<span class="sourceLineNo">314</span>         * Returns the inverse of the camera intrinsic matrix A<a name="line.314"></a>
<span class="sourceLineNo">315</span>         * as a 3x3 matrix (without the last row {0,0,1}).<a name="line.315"></a>
<span class="sourceLineNo">316</span>         * This version uses closed form matrix inversion.<a name="line.316"></a>
<span class="sourceLineNo">317</span>         * Used for rectifying images (i.e., removing lens distortion).<a name="line.317"></a>
<span class="sourceLineNo">318</span>         * <a name="line.318"></a>
<span class="sourceLineNo">319</span>         * @return the inverse of the camera intrinsic matrix A<a name="line.319"></a>
<span class="sourceLineNo">320</span>         */<a name="line.320"></a>
<span class="sourceLineNo">321</span>        public RealMatrix getInverseA() {<a name="line.321"></a>
<span class="sourceLineNo">322</span>                double alpha = A[0][0];<a name="line.322"></a>
<span class="sourceLineNo">323</span>                double beta = A[1][1];<a name="line.323"></a>
<span class="sourceLineNo">324</span>                double gamma = A[0][1];<a name="line.324"></a>
<span class="sourceLineNo">325</span>                double uc = A[0][2];<a name="line.325"></a>
<span class="sourceLineNo">326</span>                double vc = A[1][2];<a name="line.326"></a>
<span class="sourceLineNo">327</span>                double[][] Ai = {<a name="line.327"></a>
<span class="sourceLineNo">328</span>                        {1.0/alpha, -gamma/(alpha*beta), (gamma*vc - beta*uc)/(alpha*beta)},<a name="line.328"></a>
<span class="sourceLineNo">329</span>                        {0,         1.0/beta,            -vc/beta}};<a name="line.329"></a>
<span class="sourceLineNo">330</span>                return MatrixUtils.createRealMatrix(Ai);<a name="line.330"></a>
<span class="sourceLineNo">331</span>        }<a name="line.331"></a>
<span class="sourceLineNo">332</span>        <a name="line.332"></a>
<span class="sourceLineNo">333</span>        /**<a name="line.333"></a>
<span class="sourceLineNo">334</span>         * Returns the homography for the given view as a 3 x 3 matrix.<a name="line.334"></a>
<span class="sourceLineNo">335</span>         * @param view the extrinsic view parameters<a name="line.335"></a>
<span class="sourceLineNo">336</span>         * @return the homography matrix<a name="line.336"></a>
<span class="sourceLineNo">337</span>         */<a name="line.337"></a>
<span class="sourceLineNo">338</span>        public RealMatrix getHomography(ViewTransform view) {<a name="line.338"></a>
<span class="sourceLineNo">339</span>                RealMatrix RT = view.getRotationMatrix();<a name="line.339"></a>
<span class="sourceLineNo">340</span>                RealVector T = view.getTranslationVector();<a name="line.340"></a>
<span class="sourceLineNo">341</span>                RT.setColumnVector(2, T);<a name="line.341"></a>
<span class="sourceLineNo">342</span>                <a name="line.342"></a>
<span class="sourceLineNo">343</span>                RealMatrix AM = MatrixUtils.createRealMatrix(3, 3); <a name="line.343"></a>
<span class="sourceLineNo">344</span>                AM.setSubMatrix(A, 0, 0);<a name="line.344"></a>
<span class="sourceLineNo">345</span>                AM.setEntry(2, 2, 1);<a name="line.345"></a>
<span class="sourceLineNo">346</span>                <a name="line.346"></a>
<span class="sourceLineNo">347</span>                RealMatrix H = AM.multiply(RT); <a name="line.347"></a>
<span class="sourceLineNo">348</span>                return H.scalarMultiply(1.0 / H.getEntry(2, 2));<a name="line.348"></a>
<span class="sourceLineNo">349</span>        }<a name="line.349"></a>
<span class="sourceLineNo">350</span>        <a name="line.350"></a>
<span class="sourceLineNo">351</span>        // -------------------------------------------------------------------<a name="line.351"></a>
<span class="sourceLineNo">352</span>        <a name="line.352"></a>
<span class="sourceLineNo">353</span>        @Deprecated<a name="line.353"></a>
<span class="sourceLineNo">354</span>        public void print(String title) {<a name="line.354"></a>
<span class="sourceLineNo">355</span>                System.out.println(this.toString());<a name="line.355"></a>
<span class="sourceLineNo">356</span>        }<a name="line.356"></a>
<span class="sourceLineNo">357</span>        <a name="line.357"></a>
<span class="sourceLineNo">358</span>        @Override<a name="line.358"></a>
<span class="sourceLineNo">359</span>        public String toString() {<a name="line.359"></a>
<span class="sourceLineNo">360</span>                return String.format("alpha=%.4f, beta=%.4f, gamma=%.4f, uc=%.4f, vc=%.4f, K=%s",<a name="line.360"></a>
<span class="sourceLineNo">361</span>                                getAlpha(), getBeta(), getGamma(), getUc(), getVc(), Matrix.toString(getK()));<a name="line.361"></a>
<span class="sourceLineNo">362</span>        }<a name="line.362"></a>
<span class="sourceLineNo">363</span>        <a name="line.363"></a>
<span class="sourceLineNo">364</span>        //---------------------------------------------------------------------<a name="line.364"></a>
<span class="sourceLineNo">365</span>        <a name="line.365"></a>
<span class="sourceLineNo">366</span>        /**<a name="line.366"></a>
<span class="sourceLineNo">367</span>         * Used for testing only.<a name="line.367"></a>
<span class="sourceLineNo">368</span>         * @param args ignored<a name="line.368"></a>
<span class="sourceLineNo">369</span>         */<a name="line.369"></a>
<span class="sourceLineNo">370</span>        public static void main(String[] args) {<a name="line.370"></a>
<span class="sourceLineNo">371</span>                ViewTransform view = new ViewTransform();<a name="line.371"></a>
<span class="sourceLineNo">372</span>                <a name="line.372"></a>
<span class="sourceLineNo">373</span>                System.out.println("Camera 1:");<a name="line.373"></a>
<span class="sourceLineNo">374</span>                Camera camera1 = new Camera (<a name="line.374"></a>
<span class="sourceLineNo">375</span>                                832.5, 832.53, 0.204494,        // alpha, beta, gamma, <a name="line.375"></a>
<span class="sourceLineNo">376</span>                                303.959, 206.585,                       // c_x, c_y<a name="line.376"></a>
<span class="sourceLineNo">377</span>                                -0.228601, 0.190353);           // k0, k1<a name="line.377"></a>
<span class="sourceLineNo">378</span>                System.out.format("k0=%.4f, k1=%.4f\n", camera1.getK()[0], camera1.getK()[1]);<a name="line.378"></a>
<span class="sourceLineNo">379</span>                double[] XYZ1 = {40, 70, 800};<a name="line.379"></a>
<span class="sourceLineNo">380</span>                double[] uv1 = camera1.project(view, XYZ1);<a name="line.380"></a>
<span class="sourceLineNo">381</span>                System.out.print(Matrix.toString(XYZ1) + " -&gt; ");<a name="line.381"></a>
<span class="sourceLineNo">382</span>                System.out.format("u=%.4f, u=%.4f\n", uv1[0], uv1[1]);<a name="line.382"></a>
<span class="sourceLineNo">383</span>                <a name="line.383"></a>
<span class="sourceLineNo">384</span>                double r = 0.95;<a name="line.384"></a>
<span class="sourceLineNo">385</span>                double rr = camera1.warp(r);<a name="line.385"></a>
<span class="sourceLineNo">386</span>                System.out.format("radial distortion: r=%.4f -&gt; rr=%.4f\n", r, rr);<a name="line.386"></a>
<span class="sourceLineNo">387</span>                r = camera1.unwarp(rr);<a name="line.387"></a>
<span class="sourceLineNo">388</span>                System.out.format("inv. radial distortion: rr=%.4f -&gt; r=%.4f\n", rr, r);<a name="line.388"></a>
<span class="sourceLineNo">389</span>                <a name="line.389"></a>
<span class="sourceLineNo">390</span>                System.out.println();<a name="line.390"></a>
<span class="sourceLineNo">391</span>                <a name="line.391"></a>
<span class="sourceLineNo">392</span>                // distorted camera<a name="line.392"></a>
<span class="sourceLineNo">393</span>                System.out.println("Camera 2:");<a name="line.393"></a>
<span class="sourceLineNo">394</span>                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {<a name="line.394"></a>
<span class="sourceLineNo">395</span>                                {832.5, 0.204494, 303.959},<a name="line.395"></a>
<span class="sourceLineNo">396</span>                                {  0.0, 832.53, 206.585},<a name="line.396"></a>
<span class="sourceLineNo">397</span>                                {  0.0,   0.0,     1.0}});<a name="line.397"></a>
<span class="sourceLineNo">398</span>                Camera camera2 = new Camera(A, new double[] {-0.2, 0.190353});<a name="line.398"></a>
<span class="sourceLineNo">399</span>                System.out.format("k0=%.4f, k1=%.4f\n", camera2.getK()[0], camera2.getK()[1]);<a name="line.399"></a>
<span class="sourceLineNo">400</span>                <a name="line.400"></a>
<span class="sourceLineNo">401</span>                double[] XYZ2 = {40, 70, 800};<a name="line.401"></a>
<span class="sourceLineNo">402</span>                double[] uv2 = camera2.project(view, XYZ2);<a name="line.402"></a>
<span class="sourceLineNo">403</span>                System.out.print(Matrix.toString(XYZ2) + " -&gt; ");<a name="line.403"></a>
<span class="sourceLineNo">404</span>                System.out.format("u=%.4f, u=%.4f\n", uv2[0], uv2[1]);<a name="line.404"></a>
<span class="sourceLineNo">405</span>                <a name="line.405"></a>
<span class="sourceLineNo">406</span>                r = 0.95;<a name="line.406"></a>
<span class="sourceLineNo">407</span>                rr = camera2.warp(r);<a name="line.407"></a>
<span class="sourceLineNo">408</span>                System.out.format("radial distortion: r=%.4f -&gt; rr=%.4f\n", r, rr);<a name="line.408"></a>
<span class="sourceLineNo">409</span>                r = camera2.unwarp(rr);<a name="line.409"></a>
<span class="sourceLineNo">410</span>                System.out.format("inv. radial distortion: rr=%.4f -&gt; r=%.4f\n", rr, r);<a name="line.410"></a>
<span class="sourceLineNo">411</span>                <a name="line.411"></a>
<span class="sourceLineNo">412</span>                System.out.println("\nTesting radial lens distortion:");<a name="line.412"></a>
<span class="sourceLineNo">413</span>                double[] xy2 = {0.3, -0.7};<a name="line.413"></a>
<span class="sourceLineNo">414</span>                System.out.format("original x=%.4f, y=%.4f\n", xy2[0], xy2[1]);<a name="line.414"></a>
<span class="sourceLineNo">415</span>                double[] xy2d = camera2.warp(xy2);<a name="line.415"></a>
<span class="sourceLineNo">416</span>                System.out.format("distorted x=%.4f, y=%.4f\n", xy2d[0], xy2d[1]);<a name="line.416"></a>
<span class="sourceLineNo">417</span>                double[] xy2u = camera2.unwarp(xy2d);<a name="line.417"></a>
<span class="sourceLineNo">418</span>                System.out.format("undistorted x=%.4f, y=%.4f\n", xy2u[0], xy2u[1]);<a name="line.418"></a>
<span class="sourceLineNo">419</span>                <a name="line.419"></a>
<span class="sourceLineNo">420</span>                System.out.println("\nTesting only radial lens distortion fun:");<a name="line.420"></a>
<span class="sourceLineNo">421</span>                double ra = 0.10;<a name="line.421"></a>
<span class="sourceLineNo">422</span>                double rb = camera2.warp(ra);<a name="line.422"></a>
<span class="sourceLineNo">423</span>                double rc = camera2.unwarp(rb);<a name="line.423"></a>
<span class="sourceLineNo">424</span>                System.out.format("ra=%.4f, rb=%.4f, rc=%.4f\n", ra, rb, rc);<a name="line.424"></a>
<span class="sourceLineNo">425</span>        }<a name="line.425"></a>
<span class="sourceLineNo">426</span>        <a name="line.426"></a>
<span class="sourceLineNo">427</span>}<a name="line.427"></a>
<span class="sourceLineNo">428</span><a name="line.428"></a>
<span class="sourceLineNo">429</span><a name="line.429"></a>
<span class="sourceLineNo">430</span><a name="line.430"></a>




























































</pre>
</div>
</body>
</html>
